#!/usr/bin/env roseus

(load "package://jskpointcloud/euslisp/jskpointcloud.l")
(ros::load-ros-manifest "human_tracking")
(ros::load-ros-manifest "jskpointcloud")
(ros::load-ros-manifest "jsk_pcl_ros")

(ros::roseus "human_point_filtering")

(defvar *snapshot-req* "/cloud_snapshot/cloud_snapshot")
(defvar *eucclus-req* "/euclidean_clustering/euclidean_clustering")

(defun get-cluster ()
  (let* ((ss-req (instance jskpointcloud::CloudSnapshotRequest
			   :init :returncloud t))
	 (ss-res (ros::service-call *snapshot-req* ss-req))
	 (ec-req (instance jsk_pcl_ros::EuclideanSegmentrequest
			   :init :input (send ss-res :cloud)
			   :tolerance 0.1))
	 (ec-res (ros::service-call *eucclus-req* ec-req))
	 (3dp-lst (mapcar #'make-3dpointcloud-from-msg2 (send ec-res :output))))
    3dp-lst))

(defun same-person-filter (pos-lst frame &key (thr 1000))
  (send *tf-listener* :wait-for-transform
  	frame "/map" (ros::time 0) 2)
  (let* ((ret (list))
	 (cam-cds (send *tf-listener* :lookup-transform
			frame "/map" (ros::time 0)))
	 (wld-pos-lst
	  (mapcar #'(lambda (x) (subseq (send cam-cds :transform-vector x) 0 2)) pos-lst)))
    (print (list 'wld-pos-lst wld-pos-lst))
    (dolist (l wld-pos-lst)
      (unless (print (find-if #'(lambda (x) (< (distance l x) thr))
			      *prev-pos-lst*))
	(push (elt pos-lst (position l wld-pos-lst)) ret)
	(push l *prev-pos-lst*)))
    (nreverse ret)))

(defun behind-obstacle-filter (pos-lst &key (pr2-width/2 400) (human-size-thr 300))
  (let ((3dp-lst (get-cluster))
	(negative-ret (list))
	(ret (copy-object pos-lst)))
    (dolist (p pos-lst)
      (let* ((theta (print (atan2 (elt p 1) (elt p 0))))
	     (pl (make-plane :point (float-vector 0 0 0)
			     :normal (float-vector (sin theta) (cos theta) 0)))
	     (px (elt p 0)))
	(dolist (3dp 3dp-lst)
	  (unless (find-if #'(lambda (x) (and (< (abs (send pl :distance x)) pr2-width/2)
					      (< (elt x 0) (- px 300))))
			   (send 3dp :points))
	    (push p ret)))))
    (dolist (l negative-ret)
      (setq ret (delete l ret)))
    ret))

(defun remove-noise (lst)
  (remove-if-not
   #'(lambda (x)
       (or (< 1000 (norm (subseq x 0 2)) 10000)
           (< 800 (elt x 2) 1300))) lst))

(defun filter-cb (msg)
  (ros::ros-info "subscribed")
  (let* ((3dp (make-3dpointcloud-from-msg msg))
	 (pos-lst (remove-noise (same-person-filter (send 3dp :points) (send msg :header :frame_id))))
	 (pub-3dp (instance 3dpointcloud :init :points pos-lst))
	 (pub-msg (make-msg-from-3dpointcloud
		   pub-3dp :frame
		   (send msg :header :frame_id))))
    (when (send pub-3dp :points)
      (ros::ros-info "publish data")
      (ros::publish *pub-pc* pub-msg))))

(defvar *sub-pc* "/people_points")
;;(defvar *sub-pc* "/face_detector/faces_cloud")
(ros::subscribe *sub-pc* sensor_msgs::PointCloud #'filter-cb 1)

(defvar *pub-pc* "/people_with_socks")
(ros::advertise *pub-pc* sensor_msgs::PointCloud 1)

(setq *tf-listener* (instance ros::transform-listener :init))

(defun spin (&key (refresh t))
  (if (or refresh (not (boundp '*prev-pos-lst*))) (setq *prev-pos-lst* (list)))
  (do-until-key (ros::spin-once)))

(spin)